/*
 *  cylk_basicUtilities.c
 *  cylk
 *
 *  Created by Benjamin Heasly on 7/14/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "cylk_basicUtilities.h"

int fitLineToXYPoints (float *Y, float *X, int *pointMask, int nPoints, float *fits) {
    int i;
    float Xsqrsum=0.0,	Xsum=0.0;
	int nMaskPoints=0;
    float XYsum=0.0, Ysum=0.0;
	
    for (i=0;i<nPoints;i++){
		if (pointMask[i]) {
            nMaskPoints ++;
			Xsum += X[i];
			Ysum += Y[i];
            Xsqrsum += pow(X[i],2);
			XYsum += X[i]*Y[i];
    	}
	}
    
	/* variable to pass to sgelsy */
    long info, lda=2, ldb=2, n=2, nrhs=1;
	long ipiv[2] = {1, 1};
    float a[4], b[2];
    a[0] = Xsqrsum;
    a[1] = Xsum;
    a[2] = Xsum;
    a[3] = nMaskPoints;
	b[0] = XYsum;
	b[1] = Ysum;
	
    sgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, &info);
    fits[0] = b[0];
	fits[1] = b[1];
    return(1);
}

void multiply3By3MatrixATimesB(float *A, float *B){
	//replace A with A.B
	float tmp[9];
	int i,j;
	
	for (i=0;i<3;i++)
		for (j=0;j<3;j++)
			tmp[i*3+j] = ((A[i*3+0] * B[0*3+j]) + (A[i*3+1] * B[1*3+j]) + (A[i*3+2] * B[2*3+j]));
	
	for (i=0;i<3;i++)
		for (j=0;j<3;j++)
			A[i*3+j] = tmp[i*3+j];
}

float meanOfValues (float *vals, int nVals) {
	float mean, total=0;
	int i, count=0;

	for (i=0; i<nVals; i++) {
		if (!isnan(vals[i])) {
			total += vals[i];
			count++;
		}
	}
	mean = total / count;
	return(mean);
}

float meanOfAbsoluteValues (float *vals, int nVals) {
	float mean, total=0;
	int i, count=0;
	
	for (i=0; i<nVals; i++) {
		if (!isnan(vals[i])) {
			total += fabs(vals[i]);
			count++;
		}
	}
	mean = total / count;
	return(mean);
}

float yInterceptOfXYPointsGivenSlope (float *Y, float *X, int *pointMask, float slope, int nPoints) {
	int i, count=0;
	float intercept, sum=0;
	
	for (i=0; i<nPoints; i++) {
		if (pointMask[i]) {
			sum += Y[i] - slope*X[i];
			count++;
		}
	}
	
	if (count)
		intercept = sum / count;
	else
		intercept = 0;
	return (intercept);
}

float rawMomentOfXToPAndYToQ (float *X, int p, float dx, float *Y, int q, float dy, int nSample) {
	float total=0;
	int i;
	
	for (i=0; i<nSample; i++) {
		if (!isnan(X[i]) && !isnan(Y[i]))
			total += pow(X[i], p) * pow(Y[i], q) * dx * dy;
	}
	return (total);
}